package com.hitqz.robot.driver.util;

import cn.hutool.core.util.HexUtil;
import com.google.common.util.concurrent.RateLimiter;
import com.hitqz.robot.api.common.entity.RobotMap;
import com.hitqz.robot.api.common.entity.RobotMapPos;
import com.hitqz.robot.driver.communication.NetworkServiceThreadPool;
import com.hitqz.robot.driver.communication.TcpClient;
import com.hitqz.robot.driver.communication.TcpServer;
import com.hitqz.robot.driver.dto.*;
import com.hitqz.robot.driver.netty.handler.IpBlacklistHandler;
import com.hitqz.robot.driver.netty.handler.TrackMessageHandler;
import com.hitqz.robot.driver.netty.protocol.decoder.TrackDecoder;
import com.hitqz.robot.driver.netty.protocol.encoder.TrackEncoder;
import com.hitqz.robot.driver.netty.protocol.message.TrackMessage;
import io.netty.buffer.ByteBuf;
import io.netty.channel.Channel;
import io.netty.channel.ChannelHandlerContext;
import io.netty.channel.ChannelInitializer;
import io.netty.handler.codec.LengthFieldBasedFrameDecoder;
import lombok.extern.slf4j.Slf4j;

import java.util.*;
import java.util.concurrent.*;
import java.util.concurrent.atomic.AtomicLong;

@Slf4j
public class TrackToolkit {
    //黑名单防止编码不一致
    public static final Set<String> blacklist = Collections.synchronizedSet(new HashSet<>());
    //机器人编码
    public static String serialNumber = "";
    //机器人运行模式
    public static byte runningMode = 0;
    //机器人控制模式
    public static byte controlMode = 0;
    //机器人巡检速度 cm/s
    public static int runningSpeed = 20;
    //定位方式  rfid/mileage
    public static String locationMethod = "rfid";
    //当前所在的站点
    public static String currentStationCode = "";
    //当前前往的站点
    public static String currentTargetCode = "";
    //当前充电标志，决定了是否要充电
    public static boolean chargeFlag = false;
    //地图信息
    public static RobotMap map;
    //地图点位
    public static Map<String, RobotMapPos> posMap;
    //累计里程
    public static AtomicLong totoMileage = new AtomicLong(0);
    //默认监听嵌入式主动上传消息的端口
    private final static int defaultPort = 8081;
    //本地server
    public static TcpServer server;
    //坞站
    public static String chargerIp = "";
    public static Integer chargerPort = null;
    public static TcpClient chargerClient;
    //远程继电器
    public static String relayIp = "";
    public static Integer relayPort = null;
    public static TcpClient relayClient;
    //
    public static boolean rfidReadNetError = false;
    public static int speakerReqStatus =0;

    //获取消息结果
    public static final ConcurrentHashMap<String, CompletableFuture<Boolean>> requestMap = new ConcurrentHashMap<>();
    //消息通道
    public static ChannelHandlerContext ctx;
    //心跳令牌桶限流
    private static final RateLimiter heartbeatsLimiter = RateLimiter.create(2);
    private static final RateLimiter rfidReadLimiter = RateLimiter.create(0.5);

    public static boolean init(Integer port){
        int bindPort = port == null?  defaultPort : port;
        if (server == null){
            server = new TcpServer("track", bindPort, new ChannelInitializer<Channel>() {
                @Override
                protected void initChannel(Channel channel) throws Exception {
                    channel.pipeline().addLast("ipHandler",new IpBlacklistHandler(blacklist));
                    channel.pipeline().addLast("frameDecoder", new LengthFieldBasedFrameDecoder(1024, 2, 1, 1, 0));
                    channel.pipeline().addLast("decoder", new TrackDecoder());
                    channel.pipeline().addLast("encoder", new TrackEncoder());
                    channel.pipeline().addLast("handler", new TrackMessageHandler());
                }
            });
            NetworkServiceThreadPool.creatNetworkServerOrClient(()->{
                try {
                    server.bind();
                } catch (InterruptedException e) {
                    log.error("[track] server bind error", e);
                }
            });
        }
        return true;
    }


    /*****************************************************************************************************/

    /**
     * 远程开关机 发一次修改电平，会切换开关机状态
     * @return boolean true 表示设置成功，false 表示设置失败
     */
    public static boolean remotePowerOnOrOff() {
     
//         byte[] command = new byte[]{0x01, 0x05, 0x00, 0x00, 0x00, 0x00, (byte) 0xCD, (byte) 0xCA};
        byte[] command = new byte[]{
                0x01, 0x05, 0x00, 0x00, (byte) 0xFF, 0x00, (byte) 0x8C, 0x3A
        };
         if (relayClient!=null){
             try {
                 log.info("远程开关机");
                 relayClient.sendBinary(command);
             } catch (Exception e) {
                 log.info("[track] remotePowerOnOrOff error", e);
                 return false;
             }
         }
         return true;
    }

    /**
     * 获取机器人序列号
     * @return boolean true 表示设置成功，false 表示设置失败
     */
    public static boolean getSerialNumber(){
        log.info("[track]query serial number");
        return sendSimpleCmd((byte)0x03);
    }

    /**
     * 清除编码器值
     * 把所有轮子的编码值都清零
     * @return boolean true 表示设置成功，false 表示设置失败
     */
    public static boolean clearEncoderValue() {
        boolean result = sendSimpleCmd((byte) 0x11);
        log.info("[track]clear encoder value,{}",result);
        return result;
    }

    /**
     * 获取硬急停和软开关状态[有主动上传]
     * @return boolean true 表示发送成功，false 表示发送失败
     */
    public static StopStatusDto getStopStatus() {
        //sendSimpleCmd((byte) 0x21);
        return Optional.ofNullable((StopStatusDto)TrackCacheUtil.get(TrackCacheUtil.KEY_STOP_STATUS)).orElse(StopStatusDto.builder().build());
    }

    /**
     * 获取所有超声波数据
     * 触发时打印日志用
     * @return boolean true 表示发送成功，false 表示发送失败
     */
    public static boolean getAllUltrasonicData() {
        return sendSimpleCmd((byte)0x24);
    }

    /**
     * 设置超声波距离
     * @param distances 超声波距离数组 触发距离和解除距离
     * @return boolean true 表示发送成功，false 表示发送失败
     */
    public static boolean setUltrasonicDistance(short[] distances) {
        byte cmd = 0x26;
        if (distances.length % 2 != 0){
            log.warn("[track]设置超声波距离的数组必须为偶数");
        }
        ByteBuf buffer = ctx.alloc().buffer(1 + distances.length * 2);
        try {
            buffer.writeByte(cmd);
            for (short distance : distances) {
                buffer.writeShortLE(distance);
            }
            // 获取 ByteBuf 中的数据到字节数组中
            byte[] bytes = new byte[buffer.readableBytes()];
            buffer.getBytes(0, bytes);
            TrackMessage trackMessage = new TrackMessage(bytes);
            ctx.writeAndFlush(trackMessage);
            boolean result = waitForResponse(trackMessage,false);
            log.info("[track] set ultrasonic distance:{},{}", distances, result);
            return result;
        } finally {
            // 确保 ByteBuf 在使用完后释放，防止内存泄漏
            buffer.release();
        }
    }

    /**
     * 获取超声波避障距离
     * 设置界面显示用
     * @return UltrasonicDistanceConfigDto 超声波避障距离
     */
    public static UltrasonicDistanceConfigDto getUltrasonicDistance(){
        sendSimpleCmd((byte)0x27);
        return Optional.ofNullable((UltrasonicDistanceConfigDto) TrackCacheUtil.get(TrackCacheUtil.KEY_ULTRASONIC_DISTANCE)).orElse(UltrasonicDistanceConfigDto.builder().build());
    }

    /**
     * 设置避障使能与禁止
     * @param ultrasonicEnable 超声波避障使能与禁止 0为禁止避障，1为使能避障
     * @param collisionAvoidanceEnabled 防碰撞避障使能与禁止
     * @param dropAvoidanceEnabled 防跌落避障使能与禁止
     * @param infraredObstacleEnabled 红外避障使能与禁止
     * @param safetyRadarEnabled 安全雷达避障使能与禁止
     * @return boolean true 表示发送成功，false 表示发送失败
     */
    public static boolean setObstacleAvoidanceEnable(byte ultrasonicEnable,
                                                     byte collisionAvoidanceEnabled,
                                                     byte dropAvoidanceEnabled,
                                                     byte infraredObstacleEnabled,
                                                     byte safetyRadarEnabled){
        byte cmd = 0x28;
        ByteBuf buffer = ctx.alloc().buffer(6);
        try {
            buffer.writeByte(cmd);
            buffer.writeByte(ultrasonicEnable);
            buffer.writeByte(collisionAvoidanceEnabled);
            buffer.writeByte(dropAvoidanceEnabled);
            buffer.writeByte(infraredObstacleEnabled);
            buffer.writeByte(safetyRadarEnabled);
            // 获取 ByteBuf 中的数据到字节数组中
            byte[] bytes = new byte[buffer.readableBytes()];
            buffer.getBytes(0, bytes);
            TrackMessage trackMessage = new TrackMessage(bytes);
            ctx.writeAndFlush(trackMessage);
            boolean result = waitForResponse(trackMessage,false);
            log.info("[track] set obstacle avoidance enable:超{},撞{},跌{},红{},安{},{}", ultrasonicEnable, collisionAvoidanceEnabled, dropAvoidanceEnabled, infraredObstacleEnabled, safetyRadarEnabled, result);
            return result;
        } finally {
            // 确保 ByteBuf 在使用完后释放，防止内存泄漏
            buffer.release();
        }
    }

    /**
     * 获取避障使能与禁止
     * @return ObstacleAvoidanceEnableDto 避障使能状态
     */
    public static ObstacleAvoidanceEnableDto getObstacleAvoidanceEnable(){
        sendSimpleCmd((byte)0x29);
        return Optional.ofNullable((ObstacleAvoidanceEnableDto) TrackCacheUtil.get(TrackCacheUtil.KEY_OBSTACLE_AVOIDANCE_ENABLE)).orElse(ObstacleAvoidanceEnableDto.builder().build());
    }


    /**
     * 获取电池信息[有主动上传]
     * @return PowerInfoDto 电池信息
     */
    public static PowerInfoDto getPowerInfo(){
        //sendSimpleCmd((byte)0x40);
        return Optional.ofNullable((PowerInfoDto) TrackCacheUtil.get(TrackCacheUtil.KEY_POWER_INFO)).orElse(PowerInfoDto.builder().build());
    }

    /**
     * 设置充电/停止充电
     * 为了充电效率嵌入式会断驱动器的电,停止充电后需要等待30s才可以控制机器人动
     * @param cmd 0x00 停止充电，0x01 开始充电
     * @return
     */
    public static boolean setChargeStatus(byte cmd){
        byte[] date = new byte[]{0x41,cmd};
        TrackMessage trackMessage = new TrackMessage(date);
        ctx.writeAndFlush(trackMessage);
        log.info("[track]set charge status:{}",cmd);
        return waitForResponse(trackMessage,false);
    }

    /**
     * 获取充电状态[主动上传,不需要请求,不然CompletableFuture会被提前释放导致超时]
     * @return boolean true 表示发送成功，false 表示发送失败
     */
    public static byte getChargeStatus(){
        sendSimpleCmd((byte)0x42);
        return Optional.ofNullable((byte) TrackCacheUtil.get(TrackCacheUtil.KEY_CHARGE_STATUS)).orElse((byte)0);
    }

    /**
     * 获取空气质量 旧[主动上传,不需要请求,不然CompletableFuture会被提前释放导致超时]
     * sendSimpleCmd((byte)0x54);
     * 获取空气质量 新[主动上传,不需要请求,不然CompletableFuture会被提前释放导致超时]
     * endSimpleCmd((byte)0x2B,(byte)0x02);
     * @return AirQualityDto 空气质量
     */
    public static AirQualityDto getAirQuality(){
        return Optional.ofNullable((AirQualityDto) TrackCacheUtil.get(TrackCacheUtil.KEY_AIR_QUALITY)).orElse(AirQualityDto.builder().build());
    }


    /**
     * 设置灯状态
     * @param status 0x00 关闭灯，0x01 打开灯
     * @return boolean true 表示发送成功，false 表示发送失败
     */
    public static boolean setLightStatus(byte status){
        return sendSimpleCmd((byte)0x96,status);
    }

    /**
     * 获取灯状态
     * @return boolean true 表示发送成功，false 表示发送失败
     */
    public static boolean getLightStatus(){
        return sendSimpleCmd((byte)0x97);
    }

    /**
     * 获取机器人定位状态[主动上传,不需要请求,不然CompletableFuture会被提前释放导致超时]
     * @return boolean true 表示发送成功，false 表示发送失败
     */
    public static LocateStatusDto getLocationStatus(){
        //sendSimpleCmd((byte)0x64,(byte)0x01);
        return Optional.ofNullable((LocateStatusDto) TrackCacheUtil.get(TrackCacheUtil.KEY_LOCATE_STATUS)).orElse(LocateStatusDto.builder().build());
    }

    /**
     * 获取语音对讲模式
     * @return boolean true 表示发送成功，false 表示发送失败
     */
    public static boolean getSpeakerStatus(){
        return sendSimpleCmd((byte)0x64,(byte)0x02);
    }

    /**
     * 获取从动轮里程计值
     * @return boolean true 表示发送成功，false 表示发送失败
     */
    public static boolean getDrivenWheel(){
        return sendSimpleCmd((byte)0x64,(byte)0x03);
    }

    /**
     * 获取机器人运行模式 （0正常巡检模式/1标定模式 ）
     * @return boolean true 表示发送成功，false 表示发送失败
     */
    public static boolean getRunningMode(){
        return sendSimpleCmd((byte)0x64,(byte)0x04);
    }

    /**
     * 获取标定定位点信息[主动上传,不需要请求,不然CompletableFuture会被提前释放导致超时]
     * @return CalibrationPointDto 定位点信息 主要是判断有没有在范围内,有没有标定过
     */
    public static CalibrationPointDto getCalibrationPointInfo(){
        if (rfidReadLimiter.tryAcquire(3)) {
            sendSimpleCmd((byte) 0x64, (byte) 0x05);
        }
        return Optional.ofNullable((CalibrationPointDto) TrackCacheUtil.get(TrackCacheUtil.KEY_CALIBRATION_POINT_INFO)).orElse(CalibrationPointDto.builder().build());
    }


    /**
     * 获取机器人控制模式
     * 0无控制模式/1上位机控制模式/2遥控器控制模式
     * 遥控器控制优先级最大一上电即为遥控器模式
     * ( 重要：机器人运行模式 0x64+0x04（0巡检模式/1标定模式）
     *        巡检模式必须在（0无控制模式）下才有效；
     *        标定模式必须在（1上位机控制模式 / 2遥控器控制模式）才有效  )
     * @return boolean true 表示发送成功，false 表示发送失败
     */
    public static boolean getControlMode(){
        return sendSimpleCmd((byte)0x64,(byte)0x06);
    }

    /**
     * 获取RFID读取功率
     * @return boolean true 表示发送成功，false 表示发送失败
     */
    public static byte getRfidReaderPower(){
        sendSimpleCmd((byte)0x64,(byte)0x07);
        return Optional.ofNullable((byte) TrackCacheUtil.get(TrackCacheUtil.KEY_RFID_READER_POWER)).orElse((byte)27);
    }

    /**
     * 获取当前所有站点信息
     * @return boolean true 表示发送成功，false 表示发送失败
     */
    public static boolean getAllStationInfo(){
        return sendSimpleCmd((byte)0x64,(byte)0x08);
    }

    /**
     * 发送心跳包
     * 机器人每1s会主动发送心跳包，收到时原报文回复即可
     */
    public static void sendHeartbeat(){
        if (ctx == null){
            return;
        }
        if (ctx.isRemoved()){
            log.warn("[track] ctx is removed");
            return;
        }
        if (heartbeatsLimiter.tryAcquire()){
            byte[] date = new byte[]{(byte)0x64,(byte)0x09};
            TrackMessage trackMessage = new TrackMessage(date);
            ctx.writeAndFlush(trackMessage);
        }
    }

    /**
     * 获取自检状态[主动上传,不需要请求,不然CompletableFuture会被提前释放导致超时]
     * @return boolean true 表示发送成功，false 表示发送失败
     */
    public static boolean getSelfCheckStatus(){
        return sendSimpleCmd((byte)0x64,(byte)0x20);
    }


    /**
     * 前往定位点
     * @param code 标定位置点编号
     * @return boolean true 表示设置成功，false 表示设置失败
     */
    public static boolean goPos(short code){
        try {
            if (controlMode != (byte) 0) {
                setControlMode((byte) 0);
                Thread.sleep(200);
            }
            if (runningMode != (byte) 0) {
                setRunningMode((byte) 0);
                Thread.sleep(200);
            }
        }catch (Exception e){
            log.info("[track]go pos change mode error",e);
        }
        //速度 cm/s
        if (code==0){
            chargeFlag = true;
        }
        int speed = Math.min(Math.max(10,runningSpeed),50);
        byte mainCmd = 0x65;
        byte subCmd = 0x01;
        ByteBuf buffer = ctx.alloc().buffer(8);
        try {
            buffer.writeByte(mainCmd);
            buffer.writeByte(subCmd);
            buffer.writeShortLE(code);
            buffer.writeIntLE(speed);
            // 获取 ByteBuf 中的数据到字节数组中
            byte[] bytes = new byte[buffer.readableBytes()];
            buffer.getBytes(0, bytes);
            TrackMessage trackMessage = new TrackMessage(bytes);
            ctx.writeAndFlush(trackMessage);
            boolean result = waitForResponse(trackMessage,true);
            log.info("[track] go pos id:{},speed:{},{}",code,speed, result);
            return result;
        } finally {
            // 确保 ByteBuf 在使用完后释放，防止内存泄漏
            buffer.release();
        }
    }


    /**
     * 设置机器人运行模式
     * (0巡检模式 /1标定模式/2上位机拷贝站点信息模式 )
     * @param mode 模式值
     * @return boolean true 表示设置成功，false 表示设置失败
     */
    public static boolean setRunningMode(byte mode){
        byte mainCmd = 0x65;
        byte subCmd = 0x04;
        ByteBuf buffer = ctx.alloc().buffer(3);
        try {
            buffer.writeByte(mainCmd);
            buffer.writeByte(subCmd);
            buffer.writeByte(mode);
            // 获取 ByteBuf 中的数据到字节数组中
            byte[] bytes = new byte[buffer.readableBytes()];
            buffer.getBytes(0, bytes);
            TrackMessage trackMessage = new TrackMessage(bytes);
            ctx.writeAndFlush(trackMessage);
            boolean result = waitForResponse(trackMessage,true);
            log.info("[track] set running mode:{},{}",mode, result);
            return result;
        } finally {
            // 确保 ByteBuf 在使用完后释放，防止内存泄漏
            buffer.release();
        }
    }

    /**
     * 设置标定定位点编号,即标定点位
     * （ 根据65+04指令，在标定模式0x01，增加定点操作必须检测到定位开关，或者 RFID 定位传感器值才可标定成功 ；
     * 65+04指令，在上位机拷贝站点信息模式0x02，可进行删除定点操作 ）
     * @param addOrDel 0删除，1增加
     * @param point
     * number 标定编号 起始点为0，定点编号从1开始编
     * mileage 标定里程，单位：mm
     * rfid RFID ID编码 12字节
     * isStart 是否为起始点，0否，1是，2标定点（用来校正里程值）
     * @return boolean true 表示设置成功，false 表示设置失败
     */
    public static boolean setCalibrationPointNumber(byte addOrDel, AddCalibrationPointDto point){
        byte mainCmd = 0x65;
        byte subCmd = 0x05;
        ByteBuf buffer = ctx.alloc().buffer(22);
        try {
            buffer.writeByte(mainCmd);
            buffer.writeByte(subCmd);
            buffer.writeByte(addOrDel);
            buffer.writeShortLE(point.getNumber());
            buffer.writeIntLE(point.getMileage());
            if (point.getRfid().length()!=24){
                log.warn("[track]rfid label {} is wrong",point.getRfid());
            }
            byte[] rfidBytes = HexUtil.decodeHex(point.getRfid());
            buffer.writeBytes(rfidBytes);
            buffer.writeByte(point.getIsStart());
            // 获取 ByteBuf 中的数据到字节数组中
            byte[] bytes = new byte[buffer.readableBytes()];
            buffer.getBytes(0, bytes);
            TrackMessage trackMessage = new TrackMessage(bytes);
            ctx.writeAndFlush(trackMessage);
            boolean result = waitForResponse(trackMessage,true);
            log.info("[track] {} calibration point:number:{},mileage:{},rfid:{},isStart:{},{}",addOrDel==(byte)0?"delete":"add",point.getNumber(),point.getMileage(),point.getRfid(), point.getIsStart(),result);
            return result;
        } finally {
            // 确保 ByteBuf 在使用完后释放，防止内存泄漏
            buffer.release();
        }
    }

    /**
     * 设置控制模式
     * @param mode (0无控制模式 /1上位机控制模式 /2遥控器模式/)
     * @return true 表示设置成功，false 表示设置失败
     */
    public static boolean setControlMode(byte mode){
        byte mainCmd = 0x65;
        byte subCmd = 0x06;
        ByteBuf buffer = ctx.alloc().buffer(3);
        try {
            buffer.writeByte(mainCmd);
            buffer.writeByte(subCmd);
            buffer.writeByte(mode);
            // 获取 ByteBuf 中的数据到字节数组中
            byte[] bytes = new byte[buffer.readableBytes()];
            buffer.getBytes(0, bytes);
            TrackMessage trackMessage = new TrackMessage(bytes);
            ctx.writeAndFlush(trackMessage);
            boolean result = waitForResponse(trackMessage,true);
            log.info("[track] set control mode:{},{}",mode, result);
            return result;
        } finally {
            // 确保 ByteBuf 在使用完后释放，防止内存泄漏
            buffer.release();
        }
    }

    /**
     * 设置RFID读取功率
     * @param value 功率dbm
     * @return true 表示设置成功，false 表示设置失败
     */
    public static boolean setRfidReaderPower(byte value){
        byte mainCmd = 0x65;
        byte subCmd = 0x07;
        ByteBuf buffer = ctx.alloc().buffer(3);
        try {
            buffer.writeByte(mainCmd);
            buffer.writeByte(subCmd);
            buffer.writeByte(value);
            // 获取 ByteBuf 中的数据到字节数组中
            byte[] bytes = new byte[buffer.readableBytes()];
            buffer.getBytes(0, bytes);
            TrackMessage trackMessage = new TrackMessage(bytes);
            ctx.writeAndFlush(trackMessage);
            boolean result = waitForResponse(trackMessage,true);
            log.info("[track] set rfid reader power mode:{},{}",value, result);
            return result;
        } finally {
            // 确保 ByteBuf 在使用完后释放，防止内存泄漏
            buffer.release();
        }
    }

    /**
     * 上位机拷贝站点信息,即批量标定
     *
     */
    public static boolean setCalibrationPointBatch(List<AddCalibrationPointDto> list){
        byte mainCmd = 0x65;
        byte subCmd = 0x08;
        ByteBuf buffer = ctx.alloc().buffer(4+list.size()*19);
        try {
            buffer.writeByte(mainCmd);
            buffer.writeByte(subCmd);
            buffer.writeShortLE(list.size());
            //list 按 number 升序
            list.sort(Comparator.comparing(AddCalibrationPointDto::getNumber));
            StringBuilder sb = new StringBuilder();
            for (AddCalibrationPointDto point : list) {
                buffer.writeShortLE(point.getNumber());
                buffer.writeIntLE(point.getMileage());
                if (point.getRfid().length()!=24){
                    log.warn("[track]rfid label {} is wrong",point.getRfid());
                    return false;
                }
                byte[] bytes = HexUtil.decodeHex(point.getRfid());
                buffer.writeBytes(bytes);
                buffer.writeByte(point.getIsStart());
                sb.append("\n").append(point);
            }
            // 获取 ByteBuf 中的数据到字节数组中
            byte[] bytes = new byte[buffer.readableBytes()];
            buffer.getBytes(0, bytes);
            TrackMessage trackMessage = new TrackMessage(bytes);
            ctx.writeAndFlush(trackMessage);
            boolean result = waitForResponse(trackMessage,true);
            log.info("[track] add calibration point batch,{},{}",result,sb);
            return result;
        } finally {
            // 确保 ByteBuf 在使用完后释放，防止内存泄漏
            buffer.release();
        }
    }

    /**
     * 清空嵌入式板所有标定点位
     * @return true 表示发送成功，false 表示发送失败
     */
    public static boolean clearCalibrationPointAll(){
        byte[] date = new byte[]{0x65,0x11,0x01};
        TrackMessage trackMessage = new TrackMessage(date);
        ctx.writeAndFlush(trackMessage);
        return waitForResponse(trackMessage,true);
    }

    /**
     * 前往指定里程
     * @param mileage 目标里程
     * @return true 表示设置成功，false 表示设置失败
     */
    public static boolean goMileage(int mileage){
        byte mainCmd = 0x65;
        byte subCmd = 0x26;
        ByteBuf buffer = ctx.alloc().buffer(6);
        try {
            buffer.writeByte(mainCmd);
            buffer.writeByte(subCmd);
            buffer.writeIntLE(mileage);
            // 获取 ByteBuf 中的数据到字节数组中
            byte[] bytes = new byte[buffer.readableBytes()];
            buffer.getBytes(0, bytes);
            TrackMessage trackMessage = new TrackMessage(bytes);
            ctx.writeAndFlush(trackMessage);
            boolean result = waitForResponse(trackMessage,true);
            log.info("[track] go mileage:{}mm,{}",mileage,result);
            return result;
        }finally {
            buffer.release();
        }
    }

    /**
     * 设置线速度和角速度
     * 注意要在遥控模式（0x65+0x04命令，参数2）下设置才有效
     * @param lineSpeed 线速度（m/s）放大100倍
     * @param angle 角度（rad）放大100倍
     */
    public static void setSpeed(short lineSpeed,short angle){
        byte cmd = 0x16;
        ByteBuf buffer = ctx.alloc().buffer(6);
        try {
            buffer.writeByte(cmd);
            buffer.writeShortLE(lineSpeed);
            buffer.writeShortLE(angle);
            // 获取 ByteBuf 中的数据到字节数组中
            byte[] bytes = new byte[buffer.readableBytes()];
            buffer.getBytes(0, bytes);
            TrackMessage trackMessage = new TrackMessage(bytes);
            ctx.writeAndFlush(trackMessage);
            //高频调用不需要等待响应
            log.info("[track] set lineSpeed:{}cm/s,angle:{}rad",lineSpeed,angle);
        }finally {
            buffer.release();
        }
    }

    /**
     * 设置里程计标定系数
     * @param type 设定标定指令 0x01机器人自动计算标定系数，0x02上位机设定标定系数
     * @param factor 乘积系数 默认值1000，数值扩大1000倍 正常范围500-1000
     * @return true 表示设置成功，false 表示设置失败
     */
    public static boolean setMileageCalibrationFactor(byte type,int factor){
        if (factor<500 || factor>1000){
            log.warn("[track]set mileage calibration factor :{} out of normal value",factor);
            return false;
        }
        byte mainCmd = 0x65;
        byte subCmd = 0x17;
        ByteBuf buffer = ctx.alloc().buffer(7);
        try {
            buffer.writeByte(mainCmd);
            buffer.writeByte(subCmd);
            buffer.writeByte(type);
            buffer.writeIntLE(factor);
            // 获取 ByteBuf 中的数据到字节数组中
            byte[] bytes = new byte[buffer.readableBytes()];
            buffer.getBytes(0, bytes);
            TrackMessage trackMessage = new TrackMessage(bytes);
            ctx.writeAndFlush(trackMessage);
            boolean result = waitForResponse(trackMessage,true);
            log.info("[track] set mileage calibration factor:{},{}",type,factor);
            return result;
        }finally {
            buffer.release();
        }
    }

    /**
     * 获取里程计标定系数
     * @return 返回标定系数
     */
    public static int getMileageCalibrationFactor(){
        sendSimpleCmd((byte)0x64,(byte)0x17);
        return Optional.ofNullable((int)TrackCacheUtil.get(TrackCacheUtil.KEY_MILEAGE_CALIBRATION_FACTOR)).orElse(1000);
    }

    /**
     * 设置最大运行速度阈值-超过了嵌入式那边会声音报警
     * @param speed 速度（m/s）放大100倍
     * @return true 表示设置成功，false 表示设置失败
     */
    public static boolean setPatrolLimitSpeed(int speed){
        byte mainCmd = 0x65;
        byte subCmd = 0x23;
        ByteBuf buffer = ctx.alloc().buffer(6);
        try {
            buffer.writeByte(mainCmd);
            buffer.writeByte(subCmd);
            buffer.writeIntLE(speed);
            // 获取 ByteBuf 中的数据到字节数组中
            byte[] bytes = new byte[buffer.readableBytes()];
            buffer.getBytes(0, bytes);
            TrackMessage trackMessage = new TrackMessage(bytes);
            ctx.writeAndFlush(trackMessage);
            boolean result = waitForResponse(trackMessage,true);
            log.info("[track] set patrol limit speed:{}cm/s",speed);
            return result;
        }finally {
            buffer.release();
        }
    }


    /**
     * 获取最大运行速度阈值
     * @return true 表示发送成功，false 表示发送失败
     */
    public static boolean getPatrolLimitSpeed(){
        return sendSimpleCmd((byte)0x64,(byte)0x23);
    }

    /**
     * 设置气体传感器使能
     * @param methaneEnable 甲烷使能（1使能，0禁能）
     * @param smokeEnable 烟雾使能（1使能，0禁能）
     * @param speakerEnable 喇叭使能（1使能，0禁能）
     * @return true 表示设置成功，false 表示设置失败
     */
    public static boolean setAirSensorEnable(byte methaneEnable,byte smokeEnable,byte speakerEnable){
        byte mainCmd = 0x65;
        byte subCmd = 0x24;
        ByteBuf buffer = ctx.alloc().buffer(10);
        try {
            buffer.writeByte(mainCmd);
            buffer.writeByte(subCmd);
            buffer.writeByte(methaneEnable);
            buffer.writeByte(smokeEnable);
            buffer.writeByte(speakerEnable);
            byte[] remain=new byte[]{0,0,0,0,0};
            buffer.writeBytes(remain);
            // 获取 ByteBuf 中的数据到字节数组中
            byte[] bytes = new byte[buffer.readableBytes()];
            buffer.getBytes(0, bytes);
            TrackMessage trackMessage = new TrackMessage(bytes);
            ctx.writeAndFlush(trackMessage);
            boolean result = waitForResponse(trackMessage,true);
            log.info("[track] set air enable:methane:{},smoke:{},speaker:{}",methaneEnable,smokeEnable,speakerEnable);
            return result;
        }finally {
            buffer.release();
        }
    }


    public static boolean getAirSensorEnable(){
        return sendSimpleCmd((byte)0x64,(byte)0x24);
    }

    /**
     * 设置甲烷报警阈值
     * @param methaneAlarmValue 甲烷报警阈值
     * @return true 表示设置成功，false 表示设置失败
     */
    public static boolean setAirSensorAlarm(int methaneAlarmValue){
        byte mainCmd = 0x65;
        byte subCmd = 0x25;
        ByteBuf buffer = ctx.alloc().buffer(18);
        try {
            buffer.writeByte(mainCmd);
            buffer.writeByte(subCmd);
            buffer.writeIntLE(methaneAlarmValue);
            buffer.writeIntLE(0);
            buffer.writeIntLE(0);
            buffer.writeIntLE(0);
            // 获取 ByteBuf 中的数据到字节数组中
            byte[] bytes = new byte[buffer.readableBytes()];
            buffer.getBytes(0, bytes);
            TrackMessage trackMessage = new TrackMessage(bytes);
            ctx.writeAndFlush(trackMessage);
            boolean result = waitForResponse(trackMessage,true);
            log.info("[track] set alarm value:methane:{}",methaneAlarmValue);
            return result;
        }finally {
            buffer.release();
        }
    }



/*****************************************************************************************************/



    /**
     * 发送数据位只有一个主命令字节的命令
     * @param cmd 指令字节
     * @return 返回是否成功发送指令
     */
    private static boolean sendSimpleCmd(byte cmd){
        byte[] date = new byte[]{cmd};
        TrackMessage trackMessage = new TrackMessage(date);
        ctx.writeAndFlush(trackMessage);
        return waitForResponse(trackMessage,false);
    }

    /**
     * 发送数据位只有主命令和子命令字节的命令
     * @param mainCmd 主命令字节
     * @param subCmd 子命令字节
     * @return 返回是否成功发送指令
     */
    private static boolean sendSimpleCmd(byte mainCmd,byte subCmd){
        byte[] date = new byte[]{mainCmd,subCmd};
        TrackMessage trackMessage = new TrackMessage(date);
        ctx.writeAndFlush(trackMessage);
        return waitForResponse(trackMessage,true);
    }


    /**
     * 将字节数组转为十六进制字符串
     * @param bytes 字节数组
     * @return 十六进制字符串
     */
    public static String bytesToHexString(byte[] bytes) {
        StringBuilder hexString = new StringBuilder();
        for (int i = 0; i < bytes.length; i++) { // 将字节转为无符号的十六进制字符串
            String hex = String.format("%02X", bytes[i] & 0xFF);
            hexString.append(hex);
            if (i < bytes.length - 1) {
                hexString.append(" ");
            }
        }
        return hexString.toString();
    }

    public static boolean waitForResponse(TrackMessage trackMessage,boolean hasSubCmd) {
        CompletableFuture<Boolean> future = new CompletableFuture<>();
        byte[] data = trackMessage.getData();
        String hexKey;
        if (hasSubCmd){
            //有子命令的
            hexKey = String.format("%02X%02X", data[0], data[1]);
        }else {
            //没有子命令的
            hexKey = String.format("%02X", data[0]);
        }
        requestMap.put(hexKey, future);
        try {
            return future.get(2, TimeUnit.SECONDS);
        } catch (InterruptedException | ExecutionException e) {
            throw new RuntimeException(e);
        } catch (TimeoutException e) {
            log.warn("[track]cmd:{} send timeout",trackMessage.getCmdHexStr());
        }
        return false;
    }

    public static void release(TrackMessage trackMessage,boolean hasSubCmd,boolean result){
        byte[] data = trackMessage.getData();
        String hexKey;
        if (hasSubCmd){
            //有子命令的
            hexKey = String.format("%02X%02X", data[0], data[1]);
        }else {
            //没有子命令的
            hexKey = String.format("%02X", data[0]);
        }
        CompletableFuture<Boolean> future = requestMap.remove(hexKey);
        if (future != null) {
            future.complete(result);
            log.debug("[track]cmd:{},release",trackMessage.getCmdHexStr());
        }
    }
}
